#include "encoder.h"
#include "driver/gpio.h"
#include "esp_log.h"
#include <string.h>
#define TAG     "encoder"
#define PCNT_HIGH_LIMIT     1000
#define PCNT_LOW_LIMIT      -1000

//短整型大小端互换
#define BigLittleSwap16(A) ((((uint16_t)(A) & 0xff00) >> 8 ) | \
                            (((uint16_t)(A) & 0x00ff) << 8 ))

//长整型大小端互换
#define BigLittleSwap32(A) ((((uint32_t)(A) & 0xff000000) >> 24) | \
                            (((uint32_t)(A) & 0x00ff0000) >> 8 ) | \
                            (((uint32_t)(A) & 0x0000ff00) << 8 ) | \
                            (((uint32_t)(A) & 0x000000ff) << 24))

int Encoder_Base::init(int a_pin, int b_pin)
{
    pcnt_unit_config_t unit_cnfig;
    memset(&unit_cnfig, 0, sizeof(pcnt_unit_config_t));
    unit_cnfig.high_limit = PCNT_HIGH_LIMIT;
    unit_cnfig.low_limit = PCNT_LOW_LIMIT;

    ESP_ERROR_CHECK(pcnt_new_unit(&unit_cnfig, &pcnt_unit_handle));

    pcnt_glitch_filter_config_t filter_config = {0};
    filter_config.max_glitch_ns = 1000;

    ESP_ERROR_CHECK(pcnt_unit_set_glitch_filter(pcnt_unit_handle, &filter_config));

    pcnt_chan_config_t chan_a_config;
    memset(&chan_a_config, 0, sizeof(pcnt_chan_config_t));
    chan_a_config.edge_gpio_num = a_pin;
    chan_a_config.level_gpio_num = a_pin;

    pcnt_chan_config_t chan_b_config;
    memset(&chan_b_config, 0, sizeof(pcnt_chan_config_t));
    chan_b_config.edge_gpio_num = b_pin;
    chan_b_config.level_gpio_num = b_pin;

    ESP_ERROR_CHECK(pcnt_new_channel(pcnt_unit_handle, &chan_a_config, &pcnt_chan_a_handle));
    ESP_ERROR_CHECK(pcnt_new_channel(pcnt_unit_handle, &chan_b_config, &pcnt_chan_b_handle));

    ESP_ERROR_CHECK(pcnt_channel_set_edge_action(pcnt_chan_a_handle, PCNT_CHANNEL_EDGE_ACTION_DECREASE, PCNT_CHANNEL_EDGE_ACTION_INCREASE));
    ESP_ERROR_CHECK(pcnt_channel_set_level_action(pcnt_chan_a_handle, PCNT_CHANNEL_LEVEL_ACTION_KEEP, PCNT_CHANNEL_LEVEL_ACTION_INVERSE));
    ESP_ERROR_CHECK(pcnt_channel_set_edge_action(pcnt_chan_b_handle, PCNT_CHANNEL_EDGE_ACTION_INCREASE, PCNT_CHANNEL_EDGE_ACTION_DECREASE));
    ESP_ERROR_CHECK(pcnt_channel_set_level_action(pcnt_chan_b_handle, PCNT_CHANNEL_LEVEL_ACTION_KEEP, PCNT_CHANNEL_LEVEL_ACTION_INVERSE));

    return 0;
}

bool Encoder_Base::pcnt_on_reach_callback(pcnt_unit_handle_t unit, const pcnt_watch_event_data_t *edata, void *user_ctx)
{
    BaseType_t high_task_wakeup;
    QueueHandle_t queue = (QueueHandle_t)user_ctx;
    on_reach_push_msg_t msg;
    msg.type = 1;
    msg.event_value = edata->watch_point_value;
    xQueueSendFromISR(queue, &(msg), &high_task_wakeup);
    return(high_task_wakeup == pdTRUE);
}

void Encoder_Base::pcnt_on_reach_task(void *argv)
{
    Encoder_Base *base = (Encoder_Base *)argv;
    int _pulse_event = 0;
    on_reach_push_msg_t msg;
    while(1) {
        if (xQueueReceive(base->queue_handle, &msg, pdMS_TO_TICKS(1000))) {
            if (msg.type == 0) {
                vTaskDelete(base->on_reach_task_handle);
            }else {
                ESP_LOGI(TAG, "Watch point event, count: %d", msg.event_value);
            }
        } else {
            ESP_ERROR_CHECK(pcnt_unit_get_count(base->pcnt_unit_handle, &_pulse_event));
            ESP_LOGI(TAG, "pluse event, count: %d", _pulse_event);
        }

    }
}

int Encoder_Base::run(bool need_watch)
{
    if (need_watch) {
        int watch_points[] = {PCNT_LOW_LIMIT, PCNT_LOW_LIMIT / 2, 0, PCNT_HIGH_LIMIT/ 2, PCNT_HIGH_LIMIT};
        for (size_t i = 0; i < sizeof(watch_points) / sizeof(watch_points[0]); i++) {
            ESP_ERROR_CHECK(pcnt_unit_add_watch_point(pcnt_unit_handle, watch_points[i]));
        }
        pcnt_event_callbacks_t cbs;
        cbs.on_reach = pcnt_on_reach_callback;
        if (!queue_handle) {
            queue_handle = xQueueCreate(10, sizeof(int));
            ESP_ERROR_CHECK(pcnt_unit_register_event_callbacks(pcnt_unit_handle, &cbs, queue_handle));
        }

        xTaskCreate(pcnt_on_reach_task, "pcnt_on_reach_task", 2048, this, 4, &on_reach_task_handle);
    }
    ESP_ERROR_CHECK(pcnt_unit_enable(pcnt_unit_handle));
    ESP_ERROR_CHECK(pcnt_unit_start(pcnt_unit_handle));
    return 0;
}

int Encoder_Base::stop(void)
{
    ESP_ERROR_CHECK(pcnt_unit_stop(pcnt_unit_handle));
    ESP_ERROR_CHECK(pcnt_unit_disable(pcnt_unit_handle));
    return 0;
}

int Encoder_Base::clear(void)
{
    pluse_count = 0;
    ESP_ERROR_CHECK(pcnt_unit_clear_count(pcnt_unit_handle));
    return 0;
}

int Encoder_Base::get_pulse_count(void)
{
    ESP_ERROR_CHECK(pcnt_unit_get_count(pcnt_unit_handle, &pluse_count));
    return pluse_count;
}

int Encoder_Base::destroy(void)
{
    stop();
    if (on_reach_task_handle) {
        on_reach_push_msg_t msg;
        msg.type = 0;
        msg.event_value = 0;
        xQueueSend(queue_handle, &msg, 0);
    }
    return 0;
}

#define MT6701_CLK_FREQ         (10*1000*1000)
#define MT6701_INPUT_DELAY_NS   ((1000*1000*1000/MT6701_CLK_FREQ)/2+20)

int Encoder_MT6701::init(int spi_bus, int spi_sck, int spi_miso, int cs_pin)
{
    spi_bus_config_t buscfg;
    memset(&buscfg, 0, sizeof(spi_bus_config_t));

    m_cs_pin = cs_pin;
    buscfg.miso_io_num = spi_miso;
    buscfg.mosi_io_num = -1;
    buscfg.sclk_io_num = spi_sck;
    buscfg.quadhd_io_num = -1;
    buscfg.quadwp_io_num = -1;
    buscfg.max_transfer_sz = 32;
    m_spi_bus = (spi_host_device_t)spi_bus;
    ESP_ERROR_CHECK(spi_bus_initialize((spi_host_device_t)m_spi_bus, &buscfg, SPI_DMA_DISABLED));

    spi_device_interface_config_t devcfg;
    memset(&devcfg, 0, sizeof(spi_device_interface_config_t));
    devcfg.command_bits = 0;
    devcfg.clock_speed_hz = MT6701_CLK_FREQ;
    devcfg.mode = 1;
    devcfg.spics_io_num = -1;
    devcfg.queue_size = 1;
    devcfg.flags = SPI_DEVICE_HALFDUPLEX | SPI_DEVICE_POSITIVE_CS;
    devcfg.input_delay_ns = MT6701_INPUT_DELAY_NS;

    ESP_ERROR_CHECK(spi_bus_add_device(m_spi_bus, &devcfg, &m_spi_dev));

    gpio_set_level((gpio_num_t)m_cs_pin, 1);
    gpio_set_direction((gpio_num_t)m_cs_pin, GPIO_MODE_OUTPUT);
    return 0;
}

void Encoder_MT6701::run()
{
}

int Encoder_MT6701::raw_code_read(mt6701_raw_data_t *data)
{
    spi_transaction_t t;
    memset(&t, 0, sizeof(spi_transaction_t));
    t.cmd = 0xff;
    t.rxlength = 24;
    t.flags = SPI_TRANS_USE_RXDATA,
    t.user = nullptr;
    gpio_set_level((gpio_num_t)m_cs_pin, 0);
    esp_err_t err = spi_device_polling_transmit(m_spi_dev, &t);
    gpio_set_level((gpio_num_t)m_cs_pin, 1);
    // ESP_LOGI(TAG, "raw_code_read ret [%d]", err);
    // ESP_LOG_BUFFER_HEX(TAG, t.rx_data, 4);
    if (err != ESP_OK) {
        return err;
    }
    // memcpy(data, t.rx_data, t.rxlength / 8);
    uint16_t temp = 0;
    memcpy(&temp, t.rx_data, 2);
    data->angle_data = BigLittleSwap16(temp) >> 2;
    memcpy(&temp, t.rx_data + 1, 2);
    data->crc = temp & 0x3f;
    data->status = (temp & 0x3c0) >> 6;

    return err;
}

uint16_t Encoder_MT6701::raw_code_read(void)
{
    mt6701_raw_data_t raw_data;
    raw_code_read(&raw_data);
    return raw_data.angle_data;
}



int Encoder_MT6701::read(mt6701_angle_data_t *data)
{
    float angle_f = 0.0f;
    if (!data) {
        return ESP_FAIL;
    }
    mt6701_raw_data_t raw_data;
    int ret = raw_code_read(&raw_data);
    data->raw_angle = raw_data.angle_data;
    
    angle_f = (float)raw_data.angle_data * (360.0f / 16384.0f);
    data->raw_angle = angle_f;
    data->filed_status = raw_data.status & 0x3;
    return ret;
}

uint8_t Encoder_MT6701::crc6(uint8_t *data, uint8_t length)
{
    uint8_t i;  
	uint8_t crc = 0;    // Initial value  
	while(length--) {  
        crc ^= *data++; // crc ^= *data; data++;
        for (i=6; i>0; --i) { 
            if (crc & 0x20)
                crc = (crc << 1) ^ 0x03;
            else
                crc = (crc << 1);
        }
    }  
	return crc&0x3f;
}
